/*  ----------------------------------------------------------------------------
 *  Arello Mobile
 *  Fast 3d Engine
 *  ----------------------------------------------------------------------------
 * Except where otherwise noted, this work is licensed under
 * http://creativecommons.org/licenses/by/3.0
 */

#ifndef __MATH_UTL_H__
#define __MATH_UTL_H__

#undef min
#undef max


#include "Matrix.h"
#include "Vector.h"
#include "Vector2.h"
#include "Vector3.h"
#include <Color.h>

namespace liba
{
namespace math
{

template <typename T>
T power_of_two( T num )
{
	T pow(0);
	while ( num >>= 1 )
		++pow;
	return pow;
}

template<typename T>
T min( T a, T b )
{
	return a < b ? a : b;
}

template<typename T>
T max( T a, T b )
{
	return a > b ? a : b;
}

template<typename T>
T abs( T a )
{
	return a < 0 ? -a : a;
}

template<typename T>
T sgn( T a )
{
	if( a < 0 ) return -1;
	if( a > 0 ) return 1;
	return 0;
}

template<typename T>
T clamp( T value, T mi, T ma )
{
	if( value < mi )
		return mi;
	if( value > ma )
		return ma;
	return value;
}

template<typename T>
T norma( T a, T b )
{
	return abs( a - b );
}

template<typename T>
bool go_to( T * value, const T & future, const T & speed )
{
	T delta = future - *value;
	if( speed >= abs( delta ) )
	{
		*value = future;
		return true;
	}
	*value += sgn(delta) * speed;
	return false;
}

template< typename T>
T remap_into_range( T value, T from_min, T from_max, T to_min, T to_max )
{
	return (value - from_min)*(to_max - to_min)/(from_max - from_min) + to_min;
}

template<typename T>
T kronekker_symbol(T i, T j)
{
	return i == j ? T(1) : T(0);
}

template<typename T, int n>
Vector<T, n> solve_slae(const MatrixSquare<T, n> & m, const Vector<T, n> & f_orig)
{
	///"slae" stands for "System of Linear Algebraic Equations": mx = f

	Vector<T, n> f = f_orig;
	MatrixSquare<T, n> a = m;

	Vector<T, n> old_1, old_2;		//used to backup copies of two rows of the matrix
	T old_f1, old_f2;						//used to backup f[0] and f[j]
	Vector<T, n> tmp_1, tmp_2;	//used while rotating matrix
	
	old_1 = old_2 = tmp_1 = tmp_2 = Vector<T, n>::zero();
	
	// those coefficients are used in rotation method
	T root, c, s;
	int p, i, j;
	for(size_t p = 0 ; p < n-1 ; p++)
	{
		for(size_t j = p+1 ; j < n ; j++)
		{
			if((a.rc(p, p) != 0) || (a.rc(j, p) != 0))
			{
				root = sqrt(a.rc(p, p) * a.rc(p, p) + a.rc(j, p) * a.rc(j, p));
				c = a.rc(p, p) / root;
				s = -a.rc(j, p) / root;
				
				//backup two elements
				old_f1 = f[p];
				old_f2 = f[j];
				
				// making rotation of f
				f[p] = c * old_f1 - s * old_f2;
				f[j] = s * old_f1 + c * old_f2;
				
				//backup two rows
				for(i = 0 ; i < n ; i++)
				{
					old_1[i] = a.rc(p, i);
					old_2[i] = a.rc(j, i);
				}
				
				//rotate matrix
				tmp_1 = c * old_1 - s * old_2;
				tmp_2 = s * old_1 + c * old_2;
				
				//update two rows of the matrix
				for(size_t i = 0 ; i < n ; i++)
				{
					a.rc(p, i) = tmp_1[i];
					a.rc(j, i) = tmp_2[i];
				}
			}  // of if ( (...) || (...) )
		}  // of for ( j ...
	}  // of for ( p ...
	
	//find resulting vector x
	Vector<T, n> x;
	x[n-1] = f[n-1] / a.rc(n-1, n-1);
	T sum;
	for(size_t i = n-2; i >-1; i--)
	{
		sum = 0 ;
		for(size_t p = i+1 ; p < n ; p++)
		{
			sum += a.rc(i, p) * x[p];
		}
		x[i] = ( f[i] - sum ) / a.rc(i, i);
	}
	
	return x;
}


template<typename T, int n>
MatrixSquare<T, n> invert(const MatrixSquare<T, n> & r)
{
   MatrixSquare<T, n> rev;
   /* By popular definition,
	*   -1
	*  A   * A = identity
	*/

   Vector<T, n> cur_col_rev; // column of reversed matrix
   Vector<T, n> cur_col_e;   // column of identity matrix
   
   int i , j ;
   for(size_t i = 0 ; i < n ; i++)
   {
	  // define current column of E
	  for(size_t j = 0 ; j < n ; j++ )
		  cur_col_e[j] = kronekker_symbol(i, j);

	  cur_col_rev = solve_slae(r, cur_col_e);
	  
	  // define current column of A reversed
	  for(size_t j = 0;j < n ; j++)
		  rev.rc(j, i) = cur_col_rev[j] ;
   }
   return rev;
}

inline float lerpf(float start, float end, float progress)
{
	return start + (end - start) * progress;
}

inline double lerp(double start, double end, double progress)
{
	return start + (end - start) * progress;
}
	
template<typename T>
inline Vector2<T> lerp(const Vector2<T> & start, const Vector2<T> & end, float progress)
{
	return start + (end - start) * progress;
}
	
template<typename T>
inline Vector3<T> lerp(const Vector3<T> & start, const Vector3<T> & end, float progress)
{
	return start + (end - start) * progress;
}
	
template<typename T, int dim>
inline Vector<T, dim> lerp(const Vector<T, dim> & start, const Vector<T, dim> & end, float progress)
{
	return start + (end - start) * progress;
}
	
inline Color lerp(const Color & start, const Color & end, float progress)
{
	return start + (end - start) * progress;
}
	
inline float random(float minValue, float maxValue)
{
	float randomValue = (float)rand() / RAND_MAX;
	
	return minValue + (maxValue - minValue) * randomValue;
}
	
inline int random(int minValue, int maxValue)
{
	return (rand() % (maxValue - minValue + 1) + minValue);
}
	
template<typename T, int dim>
inline Vector<T, dim> random(Vector<T, dim> minValue, Vector<T, dim> maxValue)
{
	Vector<T, dim> result;
	
	for(size_t i = 0; i < dim; i++)
		result[i] = random(minValue[i], maxValue[i]);
	
	return result;
}

template<typename T>
inline Vector2<T> random(Vector2<T> minValue, Vector2<T> maxValue)
{
	return Vector2<T>(random(minValue.x, maxValue.x), random(minValue.y, maxValue.y));
}

template<typename T>
inline Vector3<T> random(Vector3<T> minValue, Vector3<T> maxValue)
{
	return Vector3<T>(random(minValue.x, maxValue.x), random(minValue.y, maxValue.y), random(minValue.z, maxValue.z));
}

	
inline Color random(const Color & minValue, const Color & maxValue)
{
	Color result = Color(random(minValue.r, maxValue.r), random(minValue.g, maxValue.g), random(minValue.b, maxValue.b));
	
	result.r = clamp(result.r, 0.0f, 1.0f);
	result.g = clamp(result.g, 0.0f, 1.0f);
	result.b = clamp(result.b, 0.0f, 1.0f);
	
	return result;
}

inline float is_nan(float f)
{
	volatile float value = f;
	return value != value;
}

inline double is_nan(double d)
{
	volatile double value = d;
	return value != value;
}
	
} // namespace math
} // namespace liba
using namespace liba;

#endif //__MATH_UTL_H__